﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using MotionPlanning.MotionPlanningPrimitives;
using MotionPlanning.MotionPlanningComponents;
using MotionPlanning.MotionPlanningComponents.Visualizations;
using MotionPlanning.MotionPlanningComponents.MotionPlanningSystems;

namespace MotionPlanning.MotionPlanningAlgorithms.RRTstar
{
    class RRTstar : Planner_RRTBased
    {
        public RRTstar(RoboticSystemComponent system, VisualizationComponent visualizer, string motionPlanningConfigFile)
            : base(system, visualizer, motionPlanningConfigFile)
        {
        }

        /// <summary>
        /// RRTstar
        /// </summary>
        /// <param name="n_iterations"></param>
        public override void Execute(int n_iterations)
        {
            if (x_root == null)
                throw new Exception("x_root uninitialized. Forget to call SetRoot()?");

            int iterations_this_round = 0;
            while (iterations_this_round < n_iterations)
            {
                ++iterations_this_round;
                ++iterations_total;
                /*
                 ----------------- Sampling Phase -------------------
                 */
                double[] q_sample = this.Sampler.NextQ(SamplingComponent.SamplingMode.SAMPLING, this.System);
                /*
                 ---------- Nearest Neighbor Search Phase -----------
                 */

                int K = 1 + (int)Math.Log(Vertices.GetSize()) * 1;
                List<object> X_near = new List<object>(100);
                this.Vertices.NearestNeighbors_KClosest_Exclusive(q_sample, K, ref X_near);

                double min_cost = double.MaxValue;
                double[] min_u = null;
                double min_step_cost = 0;
                Node min_x = null;
                for (int i = 0; i < X_near.Count; ++i)
                {
                    Node x_i = (Node)X_near[i];
                    double stepCost_i = double.MaxValue;
                    double stepDuration_i = 0;
                    double[] u_i = new double[System.Get_DOF_control() + 1];
                    this.System.Steering(x_i.get_Value().get_q(), q_sample, ref u_i, ref stepDuration_i, ref stepCost_i);
                    u_i[System.Get_DOF_control()] = stepDuration_i;

                    double cost_i = x_i.get_Value().get_utility_c() + stepCost_i;
                    if (cost_i < min_cost)
                    {
                        min_cost = cost_i;
                        min_step_cost = stepCost_i;
                        min_u = u_i;
                        min_x = x_i;
                    }
                }

                Node x_new = null;
                if (min_x == null)
                {
                    throw new Exception("this should not happen");
                }
                else
                {
                    //grow
                    x_new = this.SpawnNode_As(q_sample);
                    x_new.get_Value().set_utility_c(min_cost);
                    Edge e_new = this.SpawnEdge(min_x, x_new, min_step_cost, min_u);
                    x_new.add_ParentEdge(e_new);
                    min_x.add_ChildEdge(e_new);
                    this.Vertices.AddPoint(x_new.get_Value().get_q(), x_new);
                    //improve
                    if (true)
                    {
                        for (int i = 0; i < X_near.Count; ++i)
                        {
                            Node x_i = (Node)X_near[i];
                            if (min_x == x_i)
                                continue;
                            double stepCost_i = double.MaxValue;
                            double stepDuration_i = 0;
                            double[] u_i = new double[System.Get_DOF_control() + 1];
                            this.System.Steering(x_new.get_Value().get_q(), x_i.get_Value().get_q(), ref u_i, ref stepDuration_i, ref stepCost_i);
                            u_i[System.Get_DOF_control()] = stepDuration_i;

                            double cost_i = min_cost + stepCost_i;

                            if (cost_i < x_i.get_Value().get_utility_c())
                            {

                                //if (CheckLoop(x_i,x_new) == true)
                                //Debugger.Break();


                                Edge e_replace = this.SpawnEdge(x_new, x_i, stepCost_i, u_i);

                                Edge e_parent = x_i.get_Parents()[0];
                                Node x_parent = e_parent.get_entry();
                                x_parent.get_Children().Remove(e_parent);
                                x_i.get_Parents().Clear();

                                x_i.add_ParentEdge(e_replace);
                                x_new.add_ChildEdge(e_replace);
                                x_i.get_Value().set_utility_c(cost_i);

                                UpdateTreeCost(x_i);
                            }
                        }
                    }
                }
                //goal check
                bool DoGoalCheck = false;
                if (DoGoalCheck)
                {
                    if (GoalReached(x_new.get_Value().get_q()))
                    {
                        if (this.solution != null)
                        {
                            if (x_new.get_Value().get_utility_c() < this.solution.GetCost())
                                this.solution = new Trajectory(x_new);
                        }
                        else
                            this.solution = new Trajectory(x_new);
                        break;
                    }
                }

                //miscellaneous
                bool DoMiscellaneous = false;
                if (DoMiscellaneous)
                {
                    Thread.Sleep(1);
                }
            }
        }

        private bool CheckLoop(Node x, Node x_target)
        {
            Node x_current = x;
            while (true)
            {
                if (x_current == x_target)
                    return true;

                List<Edge> E_parent = x_current.get_Parents();

                if (E_parent == null)
                    break;
                if (E_parent.Count == 0)
                    break;
                if (E_parent.Count > 1)
                    Debugger.Break();


                x_current = E_parent[0].get_entry();


            }



            return false;
        }

        private void UpdateTreeCost(Node root)
        {
            double root_cost = root.get_Value().get_utility_c();
            List<Edge> childrenEdges = root.get_Children();
            if (childrenEdges != null)
                for (int i = 0; i < childrenEdges.Count; i++)
                {
                    Edge childEdge = childrenEdges[i];
                    Node child = childEdge.get_exit();
                    child.get_Value().set_utility_c(root_cost + childEdge.get_Value().get_utility_inc_c());
                    UpdateTreeCost(child);
                }
        }

        public override Node SpawnNode_Like(double[] q)
        {
            int DOF_state = this.System.Get_DOF_state();
            double[] q_new = new double[DOF_state];
            for (int i = 0; i < DOF_state; ++i)
                q_new[i] = q[i];
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RRTstar(q_new, 0));
            return x;
        }

        public override Node SpawnNode_As(double[] q)
        {
            if (q.Length != this.System.Get_DOF_state())
                throw new Exception("q.length != DOF_state. in RRTas.SpawnNode_As");
            Node x = Node.SpawnGraphicNode();
            x.set_Value(new NodeData_RRTstar(q, 0));
            return x;
        }
        public override Edge SpawnEdge()
        {
            Edge e = Edge.SpawnGraphicEdge();
            e.set_Value(new EdgeData_RRTstar());
            return e;
        }
        public override Edge SpawnEdge(Node entry, Node exit)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RRTstar());
            return e;
        }
        public Edge SpawnEdge(Node entry, Node exit, double c_inc, double[] ctrl_inc)
        {
            Edge e = Edge.SpawnGraphicEdge(entry, exit);
            e.set_Value(new EdgeData_RRTstar(c_inc, ctrl_inc));
            return e;
        }
        public override string ToString()
        {
            throw new NotImplementedException();
        }
        public override void Test()
        {
            throw new NotImplementedException();
        }
        override public void Plot()
        {
            List<object> graph = Vertices.GetAllDataObjects();
            Visualizer.PlotVerticesWithCost(graph);
        }
        /// <summary>
        ///  test if q_test is in the goal region
        /// </summary>
        /// <param name="q_test"></param>
        /// <returns></returns>
        public override bool GoalReached(double[] q_test)
        {
            if (r_goal <= 0)
                throw new NotImplementedException("Forget to call SetGoal and SetGoalTolerance first?");
            double distance2 = this.System.Distance2_Euclidean(q_goal, q_test);
            if (distance2 <= r_goal * r_goal)
                return true;
            else
                return false;
        }
    }
}
